/***************************************************************************
 创建者: 　　hualei
 开始时间: 2020-09-29
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.9.29 类的具体实现代码编写

 ***************************************************************************
 *  LIFT 升降机构指令具体实现
 ***************************************************************************/

#include "liftmechanisminstructionlogic.h"
#include "programengine.h"
#include "programset.h"
#include "mccontrollerthread.h"
#include "../EcatCore/robotio.h"
#include "GeneralDefine.h"
#include "messagelog.h"

//#define D_TOP_SENSOR_INDEX 8 //从０开始
//#define D_BOTTON_SENSOR_INDEX 9 //从０开始
//#define D_DRIVE_MOTOR_DO_INDEX 8 //从０开始
//#define D_DRIVE_MOTOR_HEAVY_UP_DO_INDEX 6 //从０开始
//#define D_DRIVE_MOTOR_HEAVY_DOWN_DO_INDEX 7 //从０开始
#define D_WAIT_SLEEP_TIME 10000.0

#define D_LIFT_LOGIC_FILE_PATH "globalConfig/programLogic/liftLogic.xml"

E_LIFT_STATUS LiftMechanismInstructionLogic::getLiftMechanismStatus()
{
    if(false==isRuned)
    {
        judgeCurrentStatus();
    }
    return basicStatus;
}

int LiftMechanismInstructionLogic::setLiftMechanismConfig(LiftMechanismConfig configIn)
{
    mechanismConfig=configIn;
    return writeConfigFile(configFilePath,configIn);

}

int LiftMechanismInstructionLogic::getLiftMechanismConfig(LiftMechanismConfig &configOut)
{
    configOut=mechanismConfig;
    return 1;
}

LiftMechanismInstructionLogic::LiftMechanismInstructionLogic(RobotIo *ioControllerIn, int robotIdIn,
                                ProgramSet *programSetIn, ProgramEngine *programEngineIn,
                                                             McControllerThread *programMcControllerIn)
{
    ioController = ioControllerIn;
    robotId = robotIdIn;
    programSet = programSetIn;
    programEngine = programEngineIn;
    mcController=programMcControllerIn;
    isInitialStatus=false;
    mechanismConfig.isWatchButtonDi=false;
    isButtonDiValid=true;
    isRuned=false;
    sensorIsNormalOn=false;
    isInCargoMonitor=false;

    basicStatus=E_LIFT_STATUS_MIDDLE;

    configFilePath=D_TCRCONTROLLER_DATA_PATH;
    configFilePath+=D_LIFT_LOGIC_FILE_PATH;
    QString readComent;
    int tmpReturn=readConfigFile(configFilePath,readComent);
    if(1!=tmpReturn)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3904, robotId,0,0,0,0,readComent);
    }
    else
    {
        judgeCurrentStatus();
        isInitialStatus=true;
    }
    if(true==mechanismConfig.isWatchButtonDi)
    {
        createThread();
    }
}

int LiftMechanismInstructionLogic::stop()
{
    liftStop(mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,mechanismConfig.heavyMotorReverse_index);
    liftStop(mechanismConfig.blockBType,mechanismConfig.heavyMotorPositiveB_index,mechanismConfig.heavyMotorReverseB_index);
    return 1;
}

int LiftMechanismInstructionLogic::clearAllMsg()
{
    judgeCurrentStatus();
    isInCargoMonitor=false;
}

E_PROGRAM_LOOP_RESULT LiftMechanismInstructionLogic::startLiftMechanism(int programNumIn,
                                        const LiftMechanismInstruction &instructionIn, int lineIndex)
{


    E_PROGRAM_LOOP_RESULT tmpResult=liftMechanism_run(instructionIn);
    if(E_PROGRAM_LOOP_RESULT_NORMAL!=tmpResult)
    {
        return tmpResult;
    }

    programEngine->calculateNextLineToRun(programNumIn);

    return programEngine->isTheLastInstruction(programNumIn);
}

E_PROGRAM_LOOP_RESULT LiftMechanismInstructionLogic::liftMechanism_run(const LiftMechanismInstruction
                                                                       &instructionIn)
{
    qDebug()<<"liftMechanism_run----------liftType"<<instructionIn.liftType<<"TIMEOUT"<<instructionIn.timeOutValue;
    isRuned=true;
    if(false==isInitialStatus)
    {
        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3904, robotId);
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    if(instructionIn.instructionType != LIFT)
    {
        return E_PROGRAM_LOOP_RESULT_ERROR;
    }

    int tmpWaitTimeOut;
    if(instructionIn.timeOutValue>mechanismConfig.liftTimeOut)
    {
        tmpWaitTimeOut=instructionIn.timeOutValue;
    }
    else
    {
        tmpWaitTimeOut=mechanismConfig.liftTimeOut;
    }
    qDebug()<<"tmpWaitTimeOut"<<tmpWaitTimeOut;

    E_PROGRAM_LOOP_RESULT tmpResult;
    // 上升
    if(instructionIn.liftType == E_LIFT_RISE)
    {
        qDebug()<<"上升";
        if(instructionIn.isMonitorCargo && cargoSensorDiIndex>=0)
        {
            if(false==ioController->getDi(cargoSensorDiIndex))
            {
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3912, robotId);
                basicStatus=E_LIFT_STATUS_ERROR;
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            isInCargoMonitor=true;
        }
        else
        {
            isInCargoMonitor=false;
        }
        basicStatus=E_LIFT_STATUS_MOVINGUP_TOP;
        tmpResult=blockMechanismUpDown(instructionIn.liftType,mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,mechanismConfig.heavyMotorReverse_index,
                                       mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index,mechanismConfig.heavyMiddleSensor_index,
                                   mechanismConfig.blockBType,mechanismConfig.heavyMotorPositiveB_index,mechanismConfig.heavyMotorReverseB_index,
                                   mechanismConfig.heavyTopSensorB_index,mechanismConfig.heavyButtomSensorB_index,mechanismConfig.heavyMiddleSensorB_index,
                                        tmpWaitTimeOut,mechanismConfig.liftDirectionExchange);
        if(E_PROGRAM_LOOP_RESULT_CEASE_STOP==tmpResult)
        {
            return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(E_PROGRAM_LOOP_RESULT_ERROR==tmpResult)
        {
            addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3903, robotId);
            basicStatus=E_LIFT_STATUS_ERROR;
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        if(true==mechanismConfig.isSetSheftRegion)
        {
             mcController->mcControllerInterpreter->setCurrentVehicleBorderId(1);//通过壁障类型来设置
        }
        basicStatus=E_LIFT_STATUS_TOP;


    }
    // 下降
    else if(instructionIn.liftType == E_LIFT_DOWN)
    {
        qDebug()<<"下降";
        if(instructionIn.isMonitorCargo && cargoSensorDiIndex>=0)
        {
            if(false==ioController->getDi(cargoSensorDiIndex))
            {
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3912, robotId);
                basicStatus=E_LIFT_STATUS_ERROR;
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            isInCargoMonitor=true;
        }
        else
        {
            isInCargoMonitor=false;
        }
        basicStatus=E_LIFT_STATUS_MOVINGDOWN_BUTTOM;


        tmpResult=blockMechanismUpDown(instructionIn.liftType,mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,mechanismConfig.heavyMotorReverse_index,
                                       mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index,mechanismConfig.heavyMiddleSensor_index,
                                   mechanismConfig.blockBType,mechanismConfig.heavyMotorPositiveB_index,mechanismConfig.heavyMotorReverseB_index,
                                   mechanismConfig.heavyTopSensorB_index,mechanismConfig.heavyButtomSensorB_index,mechanismConfig.heavyMiddleSensorB_index,
                                         tmpWaitTimeOut,mechanismConfig.liftDirectionExchange);
        if(E_PROGRAM_LOOP_RESULT_CEASE_STOP==tmpResult)
        {
            return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(E_PROGRAM_LOOP_RESULT_ERROR==tmpResult)
        {
            addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3903, robotId);
            basicStatus=E_LIFT_STATUS_ERROR;
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        if(true==mechanismConfig.isSetSheftRegion)
        {
             mcController->mcControllerInterpreter->setCurrentVehicleBorderId(0);//通过壁障类型来设置
        }
        basicStatus=E_LIFT_STATUS_BUTTOM;
    }
    // 上升middle
    else if(instructionIn.liftType == E_LIFT_RISE_MIDDLE)
    {
        qDebug()<<"上升middle";
        if(instructionIn.isMonitorCargo && cargoSensorDiIndex>=0)
        {
            if(false==ioController->getDi(cargoSensorDiIndex))
            {
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3912, robotId);
                basicStatus=E_LIFT_STATUS_ERROR;
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            isInCargoMonitor=true;
        }
        else
        {
            isInCargoMonitor=false;
        }
        basicStatus=E_LIFT_STATUS_MOVINGUP_MIDDLE;

        tmpResult=blockMechanismUpDown(instructionIn.liftType,mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,mechanismConfig.heavyMotorReverse_index,
                                       mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index,mechanismConfig.heavyMiddleSensor_index,
                                   mechanismConfig.blockBType,mechanismConfig.heavyMotorPositiveB_index,mechanismConfig.heavyMotorReverseB_index,
                                   mechanismConfig.heavyTopSensorB_index,mechanismConfig.heavyButtomSensorB_index,mechanismConfig.heavyMiddleSensorB_index,
                                         tmpWaitTimeOut,mechanismConfig.liftDirectionExchange);
        if(E_PROGRAM_LOOP_RESULT_CEASE_STOP==tmpResult)
        {
            return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(E_PROGRAM_LOOP_RESULT_ERROR==tmpResult)
        {
            addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3903, robotId);
            basicStatus=E_LIFT_STATUS_ERROR;
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        if(true==mechanismConfig.isSetSheftRegion)
        {
             mcController->mcControllerInterpreter->setCurrentVehicleBorderId(2);//通过壁障类型来设置
        }

        basicStatus=E_LIFT_STATUS_MIDDLE;


    }
    // 下降middle
    else if(instructionIn.liftType == E_LIFT_DOWN_MIDDLE)
    {
        qDebug()<<"下降middle";
        if(instructionIn.isMonitorCargo && cargoSensorDiIndex>=0)
        {
            if(false==ioController->getDi(cargoSensorDiIndex))
            {
                addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3912, robotId);
                basicStatus=E_LIFT_STATUS_ERROR;
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            isInCargoMonitor=true;
        }
        else
        {
            isInCargoMonitor=false;
        }
        basicStatus=E_LIFT_STATUS_MOVINGDOWN_MIDDLE;
        tmpResult=blockMechanismUpDown(instructionIn.liftType,mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,mechanismConfig.heavyMotorReverse_index,
                                       mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index,mechanismConfig.heavyMiddleSensor_index,
                                   mechanismConfig.blockBType,mechanismConfig.heavyMotorPositiveB_index,mechanismConfig.heavyMotorReverseB_index,
                                   mechanismConfig.heavyTopSensorB_index,mechanismConfig.heavyButtomSensorB_index,mechanismConfig.heavyMiddleSensorB_index,
                                         tmpWaitTimeOut,mechanismConfig.liftDirectionExchange);
        if(E_PROGRAM_LOOP_RESULT_CEASE_STOP==tmpResult)
        {
            return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
        }
        else if(E_PROGRAM_LOOP_RESULT_ERROR==tmpResult)
        {
            addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3903, robotId);
            basicStatus=E_LIFT_STATUS_ERROR;
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        if(true==mechanismConfig.isSetSheftRegion)
        {
             mcController->mcControllerInterpreter->setCurrentVehicleBorderId(0);//通过壁障类型来设置
        }
        basicStatus=E_LIFT_STATUS_MIDDLE;
    }


    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

E_PROGRAM_LOOP_RESULT LiftMechanismInstructionLogic::liftMechanism_run(const LiftParameter &paraIn)
{
    LiftMechanismInstruction tmp_instruction;
    tmp_instruction.instructionType=LIFT;
    tmp_instruction.liftType=paraIn.liftType;
    tmp_instruction.timeOutValue=paraIn.timeOutValue;
    tmp_instruction.isMonitorCargo=paraIn.isMonitorCargo;
    return liftMechanism_run(tmp_instruction);
}

int LiftMechanismInstructionLogic::setJogButtonEnable(bool isEnableIn)
{
    isButtonDiValid=isEnableIn;
    return 1;
}

void LiftMechanismInstructionLogic::judgeCurrentStatus()
{
//    qDebug()<<"LiftMechanismInstructionLogic::judgeCurrentStatus"<<ioController->getDi(mechanismConfig.heavyTopSensor_index);
    if(isInTop_block(mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index))
    {
        basicStatus=E_LIFT_STATUS_TOP;
    }
    else if(isInButtom_block(mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index))
    {
        basicStatus=E_LIFT_STATUS_BUTTOM;
    }
    else if(isInMiddle_block(mechanismConfig.heavyMiddleSensor_index,mechanismConfig.heavyButtomSensor_index
                        ,mechanismConfig.heavyTopSensor_index))
    {
        basicStatus=E_LIFT_STATUS_MIDDLE;
    }
    else
    {
        basicStatus=E_LIFT_STATUS_UNKNOW;
    }

}

//int LiftMechanismInstructionLogic::commonLiftUp()
//{
//    ioController->setDo(mechanismConfig.heavyMotorPositive_index, true);
//    qDebug()<<"commonLiftUp index"<<mechanismConfig.heavyMotorPositive_index<<" ,value true";
//    return 1;
//}

//int LiftMechanismInstructionLogic::commonLiftDown()
//{
//    ioController->setDo(mechanismConfig.heavyMotorPositive_index, true);
//    qDebug()<<"commonLiftDown index"<<mechanismConfig.heavyMotorPositive_index<<" ,value true";
//    return 1;
//}

//int LiftMechanismInstructionLogic::commonLiftStop()
//{
//    ioController->setDo(mechanismConfig.heavyMotorPositive_index, false);
//    qDebug()<<"commonLiftStop index"<<mechanismConfig.heavyMotorPositive_index<<" ,value false";
//    return 1;
//}

//int LiftMechanismInstructionLogic::heavyLiftUp()
//{
//    switch (mechanismConfig.heavyLiftDriverType) {
//    case 0:
//        heavyLiftUp_do();
//        break;
//    case 1:
//        heavyLiftUp_netDriver();
//        break;
//    case 2:

//        ioController->setDo(mechanismConfig.heavyMotorPositive_index,false );//dir 0 up 1down
//        ioController->setDo(mechanismConfig.heavyMotorReverse_index, true);//go
//        break;
//    default:
//        break;
//    }
//    return 1;
//}

//int LiftMechanismInstructionLogic::heavyLiftDown()
//{
//    switch (mechanismConfig.heavyLiftDriverType) {
//    case 0:
//        heavyLiftDown_do();
//        break;
//    case 1:
//        heavyLiftDown_netDriver();
//    case 2:
//        ioController->setDo(mechanismConfig.heavyMotorReverse_index, true);//
//        ioController->setDo(mechanismConfig.heavyMotorPositive_index, true);//
//        break;
//        break;
//    default:
//        break;
//    }
//    return 1;
//}

//int LiftMechanismInstructionLogic::heavyLiftStop()
//{
//    switch (mechanismConfig.heavyLiftDriverType) {
//    case 2:
//    case 0:
//        heavyLiftStop_do();
//        break;
//    case 1:
//        heavyLiftStop_netDriver();
//        break;
//    default:
//        break;
//    }
//    return 1;
//}

//int LiftMechanismInstructionLogic::heavyLiftUp_do()
//{
//    ioController->setDo(mechanismConfig.heavyMotorReverse_index, false);
//    ioController->setDo(mechanismConfig.heavyMotorPositive_index, true);
//    return 1;
//}

//int LiftMechanismInstructionLogic::heavyLiftDown_do()
//{

//    ioController->setDo(mechanismConfig.heavyMotorPositive_index, false);
//    ioController->setDo(mechanismConfig.heavyMotorReverse_index, true);
//    return 1;
//}

//int LiftMechanismInstructionLogic::heavyLiftStop_do()
//{
//    ioController->setDo(mechanismConfig.heavyMotorPositive_index, false);
//    ioController->setDo(mechanismConfig.heavyMotorReverse_index, false);
//    return 1;
//}

int LiftMechanismInstructionLogic::heavyLiftUp_netDriver(int axisIdIn)
{
    //如果电机速度不为０时使能，会失败。
    mcController->mcControllerInterpreter->moveOneStepByVelocity(axisIdIn,mechanismConfig.heavyLiftSpeed);
    mcController->mcControllerInterpreter->servoOn(axisIdIn);
    return 1;
}

int LiftMechanismInstructionLogic::heavyLiftDown_netDriver(int axisIdIn)
{
    //如果电机速度不为０时使能，会失败。
    mcController->mcControllerInterpreter->moveOneStepByVelocity(axisIdIn,-mechanismConfig.heavyLiftSpeed);
    mcController->mcControllerInterpreter->servoOn(axisIdIn);
    return 1;
}

int LiftMechanismInstructionLogic::heavyLiftStop_netDriver(int axisIdIn)
{
    mcController->mcControllerInterpreter->moveOneStepByVelocity(axisIdIn,0);
    mcController->mcControllerInterpreter->servoOff(axisIdIn);
    return 1;
}

int LiftMechanismInstructionLogic::readConfigFile(QString fileName,QString &readComent)
{
    qDebug() << "LiftMechanismInstructionLogic::readConfigFile ==== 初始化配置文件：" << fileName;

    DomParser domparser;
    bool ok = domparser.openXml(fileName, QIODevice::ReadOnly);
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 配置文件打开失败：" << fileName;
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyLiftSpeed", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyLiftSpeed结点失败：" << fileName;
        readComent+=" heavyLiftSpeed";
        return -3;
    }
    domparser.readXml(domNode, mechanismConfig.heavyLiftSpeed);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isSetSheftRegion", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取isSetSheftRegion结点失败：" << fileName;
        readComent+=" isSetSheftRegion";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.isSetSheftRegion);

    //block1
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "blockType", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取mechanicalType结点失败：" << fileName;
        readComent+=" blockType";
        return -2;
    }
    int tmpBlockType;
    domparser.readXml(domNode, tmpBlockType);
    mechanismConfig.blockType=(E_BLOCK_TYPE)tmpBlockType;


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyLift_axisId", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyLift_axisId结点失败：" << fileName;
        readComent+=" heavyLift_axisId";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyLift_axisId);




    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyTopSensor_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyTopSensor_index结点失败：" << fileName;
        readComent+=" heavyTopSensor_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyTopSensor_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyButtomSensor_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyButtomSensor_index结点失败：" << fileName;
        readComent+=" heavyButtomSensor_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyButtomSensor_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMiddleSensor_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyButtomSensor_index结点失败：" << fileName;
        readComent+=" heavyMiddleSensor_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyMiddleSensor_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMotorPositive_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyMotorPositive_index结点失败：" << fileName;
        readComent+=" heavyMotorPositive_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyMotorPositive_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMotorReverse_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyMotorReverse_index结点失败：" << fileName;
        readComent+=" heavyMotorReverse_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyMotorReverse_index);

    //block2
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "blockBType", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取mechanicalType结点失败：" << fileName;
        readComent+=" blockBType";
        return -2;
    }
    domparser.readXml(domNode, tmpBlockType);
    mechanismConfig.blockBType=(E_BLOCK_TYPE)tmpBlockType;


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyLiftB_axisId", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyLift_axisId结点失败：" << fileName;
        readComent+=" heavyLiftB_axisId";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyLiftB_axisId);




    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyTopSensorB_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyTopSensor_index结点失败：" << fileName;
        readComent+=" heavyTopSensorB_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyTopSensorB_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyButtomSensorB_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyButtomSensor_index结点失败：" << fileName;
        readComent+=" heavyButtomSensorB_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyButtomSensorB_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMiddleSensorB_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyButtomSensor_index结点失败：" << fileName;
        readComent+=" heavyMiddleSensorB_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyMiddleSensorB_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMotorPositiveB_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyMotorPositive_index结点失败：" << fileName;
        readComent+=" heavyMotorPositiveB_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyMotorPositiveB_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMotorReverseB_index", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取heavyMotorReverse_index结点失败：" << fileName;
        readComent+=" heavyMotorReverseB_index";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.heavyMotorReverseB_index);

    //watch di
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isWatchButtonDi", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取结点失败：" << fileName;
        readComent+=" isWatchButtonDi";
        return -4;
    }

    domparser.readXml(domNode, mechanismConfig.isWatchButtonDi);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "upButtonDiIndex", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取结点失败：" << fileName;
        readComent+=" upButtonDiIndex";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.upButtonDiIndex);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "downButtonDiIndex", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取结点失败：" << fileName;
        readComent+=" downButtonDiIndex";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.downButtonDiIndex);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "liftManualSpeed", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取结点失败：" << fileName;
        readComent+=" liftManualSpeed";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.liftManualSpeed);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "liftTimeOut", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取结点失败：" << fileName;
        readComent+=" liftTimeOut";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.liftTimeOut);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "liftDirectionExchange", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取结点失败：" << fileName;
        readComent+=" liftDirectionExchange";
        return -4;
    }
    domparser.readXml(domNode, mechanismConfig.liftDirectionExchange);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "sensorIsNormalOn", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== 读取结点失败：sensorIsNormalOn" << fileName;
        readComent+=" sensorIsNormalOn";
        return -110;
    }
    domparser.readXml(domNode, sensorIsNormalOn);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "cargoSensorDiIndex", ok );
    if( !ok )
    {
        qDebug() << "readConfigFile ==== cargoSensorDiIndex" << fileName;
        readComent+=" cargoSensorDiIndex";
        return -111;
    }
    domparser.readXml(domNode, cargoSensorDiIndex);


    domparser.closeXml();
    return 1;

}

int LiftMechanismInstructionLogic::writeConfigFile(QString fileName, LiftMechanismConfig configIn)
{
    qDebug() << "LiftMechanismInstructionLogic::writeConfigFile ==== 初始化配置文件：" << fileName;

    DomParser domparser;
    bool ok = domparser.openXml(fileName, QIODevice::ReadWrite);
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 配置文件打开失败：" << fileName;
        return -1;
    }


    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyLiftSpeed", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyLiftSpeed结点失败：" << fileName;
        return -3;
    }
    domparser.writeXml(domNode, configIn.heavyLiftSpeed);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isSetSheftRegion", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取isSetSheftRegion结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.isSetSheftRegion);

    //block1
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "blockType", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取mechanicalType结点失败：" << fileName;
        return -2;
    }
    int tmpBlock=configIn.blockType;
    domparser.writeXml(domNode,tmpBlock );



    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyLift_axisId", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyLift_axisId结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyLift_axisId);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyTopSensor_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyTopSensor_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyTopSensor_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyButtomSensor_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyButtomSensor_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyButtomSensor_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMiddleSensor_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyButtomSensor_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyMiddleSensor_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMotorPositive_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyMotorPositive_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyMotorPositive_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMotorReverse_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyMotorReverse_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyMotorReverse_index);

    //block2
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "blockBType", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取mechanicalType结点失败：" << fileName;
        return -2;
    }
    tmpBlock=configIn.blockBType;
    domparser.writeXml(domNode,tmpBlock );



    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyLiftB_axisId", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyLift_axisId结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyLiftB_axisId);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyTopSensorB_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyTopSensor_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyTopSensorB_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyButtomSensorB_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyButtomSensor_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyButtomSensorB_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMiddleSensorB_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyButtomSensor_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyMiddleSensorB_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMotorPositiveB_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyMotorPositive_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyMotorPositiveB_index);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "heavyMotorReverseB_index", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取heavyMotorReverse_index结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.heavyMotorReverseB_index);

    //watch di
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isWatchButtonDi", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取结点失败：" << fileName;
        return -4;
    }

    domparser.writeXml(domNode, configIn.isWatchButtonDi);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "upButtonDiIndex", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.upButtonDiIndex);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "downButtonDiIndex", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.downButtonDiIndex);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "liftManualSpeed", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.liftManualSpeed);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "liftTimeOut", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.liftTimeOut);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "liftDirectionExchange", ok );
    if( !ok )
    {
        qDebug() << "writeConfigFile ==== 读取结点失败：" << fileName;
        return -4;
    }
    domparser.writeXml(domNode, configIn.liftDirectionExchange);

    domparser.closeXml(true);
    return 1;
}

E_PROGRAM_LOOP_RESULT LiftMechanismInstructionLogic::blockMechanismUpDown(EM_LIFT_TYPE moveTypeIn,
                         E_BLOCK_TYPE blokType, int motorDo1Index, int motorDo2Index,
                         int topArrivalIndex, int buttomArrivalIndex, int middleArrivalIndex,
                    E_BLOCK_TYPE blokTypeB, int motorDo1IndexB, int motorDo2IndexB,
                   int topArrivalIndexB, int buttomArrivalIndexB, int middleArrivalIndexB,
                                                        double blockWaitTime, bool isExchangeDirectionIn)
{
    qDebug()<<" LIFT blockMechanismUpDown blokType"<<blokType<<"moveTypeIn"<<moveTypeIn<<"motorDo1Index"<<motorDo1Index
           <<"motorDo2Index"<<motorDo2Index<<"topArrivalIndex"<<topArrivalIndex
          <<"buttomArrivalIndex"<<buttomArrivalIndex<<"blockWaitTime"<<blockWaitTime;
    bool isFirstFinished=false;
    bool isSecondFinished=false;
    // 上升
    if(E_LIFT_RISE==moveTypeIn)
    {
        switch (blokType)
        {
        case E_BLOCK_TYPE_NOP:
             isFirstFinished=true;
            break;
        case E_BLOCK_TYPE_MAGANETIC:
            isFirstFinished=true;
            break;
        }


        switch (blokTypeB)
        {
        case E_BLOCK_TYPE_NOP:
              isSecondFinished=true;
            break;
        case E_BLOCK_TYPE_MAGANETIC:
                    isSecondFinished=true;
            break;
        }

        if(isInTop_block(topArrivalIndex,buttomArrivalIndex) && false==isFirstFinished)
        {
            isFirstFinished=true;

        }
        if(isInTop_block(topArrivalIndexB,buttomArrivalIndexB) && false==isSecondFinished)
        {
            isSecondFinished=true;

        }
        if(isSecondFinished&&isFirstFinished)
        {
            return E_PROGRAM_LOOP_RESULT_NORMAL;
        }

        liftUp(blokType,motorDo1Index,motorDo2Index,isExchangeDirectionIn);
        liftUp(blokTypeB,motorDo1IndexB,motorDo2IndexB,isExchangeDirectionIn);


        int sleepCount=0;

        while(false==isFirstFinished || false==isSecondFinished )
        {
            if(isInTop_block(topArrivalIndex,buttomArrivalIndex) && false==isFirstFinished)
            {
                isFirstFinished=true;
                liftStop(blokType,motorDo1Index,motorDo2Index);

            }
            if(isInTop_block(topArrivalIndexB,buttomArrivalIndexB) && false==isSecondFinished)
            {
                isSecondFinished=true;
                liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

            }

            realTimeSleep(D_WAIT_SLEEP_TIME);
            sleepCount++;
            double totalWaitTime=sleepCount*D_WAIT_SLEEP_TIME;
            if(totalWaitTime>blockWaitTime*1000000)
            {

                liftStop(blokType,motorDo1Index,motorDo2Index);
                liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

                qDebug()<<"error,,,blockMechanismUpDown, time out error";
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }

            //cease stop
            MC_TRAJECTORY_STATUS tmpStatus=(MC_TRAJECTORY_STATUS)mcController->mcControllerInterpreter->getTrajServerStatus();
            if(ENUM_TRAJECTORY_STOPPED == tmpStatus)
            {

                liftStop(blokType,motorDo1Index,motorDo2Index);
                liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

               return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
            }
            else if(ENUM_TRAJECTORY_TOCEASE == tmpStatus || ENUM_TRAJECTORY_CEASING == tmpStatus
                     || ENUM_TRAJECTORY_CEASED == tmpStatus)
            {

                liftStop(blokType,motorDo1Index,motorDo2Index);
                liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);
                sleepCount=0;
            }
            else if(ENUM_TRAJECTORY_RUN == tmpStatus)
            {
                if(false==isFirstFinished)
                {
                    liftUp(blokType,motorDo1Index,motorDo2Index,isExchangeDirectionIn);
                }
                if(false==isSecondFinished)
                {
                    liftUp(blokTypeB,motorDo1IndexB,motorDo2IndexB,isExchangeDirectionIn);
                }

            }

        }




    }
    // 下降
    else if(E_LIFT_DOWN==moveTypeIn)
    {

        switch (blokType)
        {
        case E_BLOCK_TYPE_NOP:
                    isFirstFinished=true;
            break;
        case E_BLOCK_TYPE_MAGANETIC:
                    isFirstFinished=true;
            break;
        }


        switch (blokTypeB)
        {
        case E_BLOCK_TYPE_NOP:
                    isSecondFinished=true;
            break;
        case E_BLOCK_TYPE_MAGANETIC:
                    isSecondFinished=true;
            break;

        }

        if(isInButtom_block(topArrivalIndex,buttomArrivalIndex) && false==isFirstFinished)
        {
            isFirstFinished=true;

        }
        if(isInButtom_block(topArrivalIndexB,buttomArrivalIndexB) && false==isSecondFinished)
        {
            isSecondFinished=true;

        }
        if(isSecondFinished&&isFirstFinished)
        {
            return E_PROGRAM_LOOP_RESULT_NORMAL;
        }
        liftDown(blokType,motorDo1Index,motorDo2Index,isExchangeDirectionIn);

        liftDown(blokTypeB,motorDo1IndexB,motorDo2IndexB,isExchangeDirectionIn);


        int sleepCount=0;
        while(false==isFirstFinished || false==isSecondFinished )
        {

            if(isInButtom_block(topArrivalIndex,buttomArrivalIndex) && false==isFirstFinished)
            {
                isFirstFinished=true;
                liftStop(blokType,motorDo1Index,motorDo2Index);

            }
            if(isInButtom_block(topArrivalIndexB,buttomArrivalIndexB) && false==isSecondFinished)
            {
                isSecondFinished=true;
                liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

            }

            realTimeSleep(D_WAIT_SLEEP_TIME);
            sleepCount++;
            double totalWaitTime=sleepCount*D_WAIT_SLEEP_TIME;
            if(totalWaitTime>blockWaitTime*1000000)
            {
                liftStop(blokType,motorDo1Index,motorDo2Index);
                liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

                qDebug()<<"blockMechanismUpDown ,down time out error";
    //                        return startJump(programNumIn,instructionIn.jmpLbValue);
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }


            //cease stop
            MC_TRAJECTORY_STATUS tmpStatus=(MC_TRAJECTORY_STATUS)mcController->mcControllerInterpreter->getTrajServerStatus();
            if(ENUM_TRAJECTORY_STOPPED == tmpStatus)
            {

                liftStop(blokType,motorDo1Index,motorDo2Index);
                liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

               return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
            }
            else if(ENUM_TRAJECTORY_TOCEASE == tmpStatus || ENUM_TRAJECTORY_CEASING == tmpStatus
                     || ENUM_TRAJECTORY_CEASED == tmpStatus)
            {

                liftStop(blokType,motorDo1Index,motorDo2Index);
                liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);
                sleepCount=0;
            }
            else if(ENUM_TRAJECTORY_RUN == tmpStatus)
            {
                if(false==isFirstFinished)
                {
                    liftDown(blokType,motorDo1Index,motorDo2Index,isExchangeDirectionIn);
                }
                if(false==isSecondFinished)
                {
                    liftDown(blokTypeB,motorDo1IndexB,motorDo2IndexB,isExchangeDirectionIn);
                }

            }


        }


    }
    else if(E_LIFT_RISE_MIDDLE==moveTypeIn)
    {// 上升middle
            switch (blokType)
            {
            case E_BLOCK_TYPE_NOP:
                 isFirstFinished=true;
                break;
            case E_BLOCK_TYPE_MAGANETIC:
                isFirstFinished=true;
                break;

            }


            switch (blokTypeB)
            {
            case E_BLOCK_TYPE_NOP:
                  isSecondFinished=true;
                break;
            case E_BLOCK_TYPE_MAGANETIC:
                        isSecondFinished=true;
                break;

            }

            if(isInMiddle_block(middleArrivalIndex,buttomArrivalIndex,topArrivalIndex) && false==isFirstFinished)
            {
                isFirstFinished=true;

            }
            if(isInMiddle_block(middleArrivalIndexB,buttomArrivalIndexB,topArrivalIndexB) && false==isSecondFinished)
            {
                isSecondFinished=true;

            }
            if(isSecondFinished&&isFirstFinished)
            {
                return E_PROGRAM_LOOP_RESULT_NORMAL;
            }

            liftUp(blokType,motorDo1Index,motorDo2Index,isExchangeDirectionIn);
            liftUp(blokTypeB,motorDo1IndexB,motorDo2IndexB,isExchangeDirectionIn);


            int sleepCount=0;

            while(false==isFirstFinished || false==isSecondFinished )
            {
                if(isInMiddle_block(middleArrivalIndex,buttomArrivalIndex,topArrivalIndex) && false==isFirstFinished)
                {
                    isFirstFinished=true;
                    liftStop(blokType,motorDo1Index,motorDo2Index);

                }
                if(isInMiddle_block(middleArrivalIndexB,buttomArrivalIndexB,topArrivalIndexB) && false==isSecondFinished)
                {
                    isSecondFinished=true;
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

                }

                realTimeSleep(D_WAIT_SLEEP_TIME);
                sleepCount++;
                double totalWaitTime=sleepCount*D_WAIT_SLEEP_TIME;
                if(totalWaitTime>blockWaitTime*1000000)
                {

                    liftStop(blokType,motorDo1Index,motorDo2Index);
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

                    qDebug()<<"error,,,blockMechanismUpDown, time out error";
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }


                //cease stop
                MC_TRAJECTORY_STATUS tmpStatus=(MC_TRAJECTORY_STATUS)mcController->mcControllerInterpreter->getTrajServerStatus();
                if(ENUM_TRAJECTORY_STOPPED == tmpStatus)
                {

                    liftStop(blokType,motorDo1Index,motorDo2Index);
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

                   return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
                }
                else if(ENUM_TRAJECTORY_TOCEASE == tmpStatus || ENUM_TRAJECTORY_CEASING == tmpStatus
                         || ENUM_TRAJECTORY_CEASED == tmpStatus)
                {

                    liftStop(blokType,motorDo1Index,motorDo2Index);
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);
                    sleepCount=0;
                }
                else if(ENUM_TRAJECTORY_RUN == tmpStatus)
                {
                    if(false==isFirstFinished)
                    {
                        liftUp(blokType,motorDo1Index,motorDo2Index,isExchangeDirectionIn);
                    }
                    if(false==isSecondFinished)
                    {
                        liftUp(blokTypeB,motorDo1IndexB,motorDo2IndexB,isExchangeDirectionIn);
                    }

                }

                if(isInTop_block(topArrivalIndex,buttomArrivalIndex) ||
                        isInTop_block(topArrivalIndexB,buttomArrivalIndexB))
                {
                    liftStop(blokType,motorDo1Index,motorDo2Index);
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);
                    qDebug()<<"arrived top error";
                   return E_PROGRAM_LOOP_RESULT_ERROR;
                }

            }




        }
        // 下降middle
    else if(E_LIFT_DOWN_MIDDLE==moveTypeIn)
        {

            switch (blokType)
            {
            case E_BLOCK_TYPE_NOP:
                        isFirstFinished=true;
                break;
            case E_BLOCK_TYPE_MAGANETIC:
                        isFirstFinished=true;
                break;
            }

            switch (blokTypeB)
            {
            case E_BLOCK_TYPE_NOP:
                        isSecondFinished=true;
                break;
            case E_BLOCK_TYPE_MAGANETIC:
                        isSecondFinished=true;
                break;

            }

            if(isInMiddle_block(middleArrivalIndex,buttomArrivalIndex,topArrivalIndex)  && false==isFirstFinished)
            {
                isFirstFinished=true;

            }
            if(isInMiddle_block(middleArrivalIndexB,buttomArrivalIndexB,topArrivalIndexB) && false==isSecondFinished)
            {
                isSecondFinished=true;
            }
//            qDebug()<<"isSecondFinished"<<isSecondFinished<<"isFirstFinished"<<isFirstFinished;
            if(true==isSecondFinished && true==isFirstFinished)
            {
//                qDebug()<<"isSecondFinished&&isFirstFinished";
                return E_PROGRAM_LOOP_RESULT_NORMAL;
            }
            liftDown(blokType,motorDo1Index,motorDo2Index,isExchangeDirectionIn);
            liftDown(blokTypeB,motorDo1IndexB,motorDo2IndexB,isExchangeDirectionIn);


            int sleepCount=0;
            while(false==isFirstFinished || false==isSecondFinished )
            {

                if(isInMiddle_block(middleArrivalIndex,buttomArrivalIndex,topArrivalIndex) && false==isFirstFinished)
                {
                    isFirstFinished=true;
                    liftStop(blokType,motorDo1Index,motorDo2Index);

                }
                if(isInMiddle_block(middleArrivalIndexB,buttomArrivalIndexB,topArrivalIndexB) && false==isSecondFinished)
                {
                    isSecondFinished=true;
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

                }

                realTimeSleep(D_WAIT_SLEEP_TIME);
                sleepCount++;
                double totalWaitTime=sleepCount*D_WAIT_SLEEP_TIME;
                if(totalWaitTime>blockWaitTime*1000000)
                {

                    liftStop(blokType,motorDo1Index,motorDo2Index);
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

                    qDebug()<<"blockMechanismUpDown ,down time out error";
        //                        return startJump(programNumIn,instructionIn.jmpLbValue);
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }


                //cease stop
                MC_TRAJECTORY_STATUS tmpStatus=(MC_TRAJECTORY_STATUS)mcController->mcControllerInterpreter->getTrajServerStatus();
                if(ENUM_TRAJECTORY_STOPPED == tmpStatus)
                {

                    liftStop(blokType,motorDo1Index,motorDo2Index);
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

                   return E_PROGRAM_LOOP_RESULT_CEASE_STOP;
                }
                else if(ENUM_TRAJECTORY_TOCEASE == tmpStatus || ENUM_TRAJECTORY_CEASING == tmpStatus
                         || ENUM_TRAJECTORY_CEASED == tmpStatus)
                {

                    liftStop(blokType,motorDo1Index,motorDo2Index);
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);
                    sleepCount=0;
                }
                else if(ENUM_TRAJECTORY_RUN == tmpStatus)
                {
                    if(false==isFirstFinished)
                    {
                        liftDown(blokType,motorDo1Index,motorDo2Index,isExchangeDirectionIn);
                    }
                    if(false==isSecondFinished)
                    {
                        liftDown(blokTypeB,motorDo1IndexB,motorDo2IndexB,isExchangeDirectionIn);
                    }

                }



                if(isInButtom_block(topArrivalIndex,buttomArrivalIndex) ||
                        isInButtom_block(topArrivalIndexB,buttomArrivalIndexB))
                {
                    liftStop(blokType,motorDo1Index,motorDo2Index);
                    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);
                    qDebug()<<"arrived buttom error";
                   return E_PROGRAM_LOOP_RESULT_ERROR;
                }

            }


        }


    liftStop(blokType,motorDo1Index,motorDo2Index);
    liftStop(blokTypeB,motorDo1IndexB,motorDo2IndexB);

    return E_PROGRAM_LOOP_RESULT_NORMAL;
}

void LiftMechanismInstructionLogic::liftUp(E_BLOCK_TYPE blokType, int motorDo1Index, int motorDo2Index,
                                           bool isExchangeDirectionIn)
{
    switch (blokType)
    {
    case E_BLOCK_TYPE_NOP:
        break;
    case E_BLOCK_TYPE_MAGANETIC:
        if(isExchangeDirectionIn)
        {
            ioController->setDo(motorDo1Index, false);
        }
        else
        {
            ioController->setDo(motorDo1Index, true);
        }

        break;
    case E_BLOCK_TYPE_DC_DRIVER:
         ioController->setDo(motorDo1Index, true);
        break;
    case E_BLOCK_TYPE_HEAVY_MOTOR:
        if(isExchangeDirectionIn)
        {
            ioController->setDo(motorDo2Index, true);
            ioController->setDo(motorDo1Index, false);
        }
        else
        {
            ioController->setDo(motorDo2Index, false);
            ioController->setDo(motorDo1Index, true);
        }

        break;
    case E_BLOCK_TYPE_HEAVY_DIR_GO:
        if(isExchangeDirectionIn)
        {
            ioController->setDo(motorDo1Index, true);//dir
            ioController->setDo(motorDo2Index, true);//go
        }
        else
        {
            ioController->setDo(motorDo1Index, false);//dir
            ioController->setDo(motorDo2Index, true);//go
        }

        break;
    case E_BLOCK_TYPE_HEAVY_CANOPEN:
                heavyLiftUp_netDriver(mechanismConfig.heavyLift_axisId);
        break;

    default:
        qDebug()<<"error5,,,blockMechanismUpDown"<<blokType;
        return ;
        break;
    }
}

void LiftMechanismInstructionLogic::liftDown(E_BLOCK_TYPE blokType, int motorDo1Index, int motorDo2Index,
                                             bool isExchangeDirectionIn)
{
    switch (blokType)
    {
    case E_BLOCK_TYPE_NOP:
        break;
    case E_BLOCK_TYPE_MAGANETIC:
        if(isExchangeDirectionIn)
        {
            ioController->setDo(motorDo1Index, true);
        }
        else
        {
            ioController->setDo(motorDo1Index, false);
        }

        break;
    case E_BLOCK_TYPE_DC_DRIVER:
         ioController->setDo(motorDo1Index, true);
        break;
    case E_BLOCK_TYPE_HEAVY_MOTOR:
        if(isExchangeDirectionIn)
        {
            ioController->setDo(motorDo2Index, false);
            ioController->setDo(motorDo1Index, true);
        }
        else
        {
            ioController->setDo(motorDo2Index, true);
            ioController->setDo(motorDo1Index, false);
        }

        break;
    case E_BLOCK_TYPE_HEAVY_DIR_GO:
        if(isExchangeDirectionIn)
        {
            ioController->setDo(motorDo1Index, false);//dir
            ioController->setDo(motorDo2Index, true);//go
        }
        else
        {
            ioController->setDo(motorDo1Index, true);//dir
            ioController->setDo(motorDo2Index, true);//go
        }

        break;
    case E_BLOCK_TYPE_HEAVY_CANOPEN:
                heavyLiftDown_netDriver(mechanismConfig.heavyLift_axisId);
        break;

    default:
        qDebug()<<"error5,,,blockMechanismUpDown"<<blokType;
        return ;
        break;
    }
}

void LiftMechanismInstructionLogic::liftStop(E_BLOCK_TYPE blokType, int motorDo1Index, int motorDo2Index)
{
    switch (blokType)
    {
    case E_BLOCK_TYPE_NOP:
        break;
    case E_BLOCK_TYPE_MAGANETIC:
        break;
    case E_BLOCK_TYPE_DC_DRIVER:
        ioController->setDo(motorDo1Index, false);
        break;
    case E_BLOCK_TYPE_HEAVY_MOTOR:
                ioController->setDo(motorDo2Index, false);
                ioController->setDo(motorDo1Index, false);
        break;
    case E_BLOCK_TYPE_HEAVY_DIR_GO:
                ioController->setDo(motorDo1Index, false);//dir
                ioController->setDo(motorDo2Index, false);//go
        break;
    case E_BLOCK_TYPE_HEAVY_CANOPEN:
                heavyLiftStop_netDriver(mechanismConfig.heavyLift_axisId);
                heavyLiftStop_netDriver(mechanismConfig.heavyLiftB_axisId);
        break;

    default:
        qDebug()<<"error5,,,blockMechanismUpDown"<<blokType;
        return ;
        break;
    }
}

//bool LiftMechanismInstructionLogic::isInTop(EM_MECHANISM_TYPE typeIn)
//{
//    switch (typeIn) {
//    case E_MECHANISM_GENERAL:
//        return isInTop_commonLift();
//        break;
//    case E_MECHANISM_HEAVY:
//        return isInTop_heavyLift();
//        break;
//    default:
//        break;
//    }
//    return false;
//}

//bool LiftMechanismInstructionLogic::isInButtom(EM_MECHANISM_TYPE typeIn)
//{
//    switch (typeIn) {
//    case E_MECHANISM_GENERAL:
//        return isInButtom_commonLift();
//        break;
//    case E_MECHANISM_HEAVY:
//        return isInButtom_heavyLift();
//        break;
//    default:
//        break;
//    }
//    return false;
//}

//bool LiftMechanismInstructionLogic::isInTop_commonLift()
//{
//    if(false==ioController->getDi(mechanismConfig.heavyTopSensor_index)
//          || true==ioController->getDi(mechanismConfig.heavyButtomSensor_index))
//    {
//        return false;
//    }
//    return true;
//}

//bool LiftMechanismInstructionLogic::isInButtom_commonLift()
//{
//    if(true==ioController->getDi(mechanismConfig.heavyTopSensor_index)
//          || false==ioController->getDi(mechanismConfig.heavyButtomSensor_index))
//    {
//        return false;
//    }
//    return true;
//}

bool LiftMechanismInstructionLogic::isInTop_block(int topArrivalIndex,int buttomArrivalIndex)
{
    if(NULL==ioController)
    {
        return false;
    }
    if(sensorIsNormalOn)
    {
        if(true==ioController->getDi(topArrivalIndex)
              || false==ioController->getDi(buttomArrivalIndex))
        {
            return false;
        }
    }
    else
    {
        if(false==ioController->getDi(topArrivalIndex)
              || true==ioController->getDi(buttomArrivalIndex))
        {
            return false;
        }
    }

    return true;
}

bool LiftMechanismInstructionLogic::isInButtom_block(int topArrivalIndex, int buttomArrivalIndex)
{
    if(NULL==ioController)
    {
        return false;
    }
    if(sensorIsNormalOn)
    {
        if(false==ioController->getDi(topArrivalIndex)
              || true==ioController->getDi(buttomArrivalIndex))
        {
            return false;
        }
    }
    else
    {
        if(true==ioController->getDi(topArrivalIndex)
              || false==ioController->getDi(buttomArrivalIndex))
        {
            return false;
        }
    }

    return true;
}

bool LiftMechanismInstructionLogic::isInMiddle_block(int middleArrivalIndex, int buttomArrivalIndex, int topArrivalIndex)
{
    if(NULL==ioController)
    {
        return false;
    }
    if(sensorIsNormalOn)
    {
        if(false==ioController->getDi(middleArrivalIndex)
              && true==ioController->getDi(buttomArrivalIndex)
                && true==ioController->getDi(topArrivalIndex))
        {
            return true;
        }
    }
    else
    {
        if(true==ioController->getDi(middleArrivalIndex)
              && false==ioController->getDi(buttomArrivalIndex)
                && false==ioController->getDi(topArrivalIndex))
        {
            return true;
        }
    }

    return false;
}


//bool LiftMechanismInstructionLogic::isInTop()
//{
//    if(false==ioController->getDi(mechanismConfig.heavyTopSensor_index)
//          || true==ioController->getDi(mechanismConfig.heavyButtomSensor_index))
//    {
//        return false;
//    }
//    return true;
//}

//bool LiftMechanismInstructionLogic::isInButtom()
//{
//    if(true==ioController->getDi(mechanismConfig.heavyTopSensor_index)
//          || false==ioController->getDi(mechanismConfig.heavyButtomSensor_index))
//    {
//        return false;
//    }
//    return true;
//}

//bool LiftMechanismInstructionLogic::isInMiddle()
//{
//    if(true==ioController->getDi(mechanismConfig.heavyMiddleSensor_index)
//          && false==ioController->getDi(mechanismConfig.heavyTopSensor_index)
//            && false==ioController->getDi(mechanismConfig.heavyButtomSensor_index))
//    {
//        return true;
//    }
//    return false;
//}


void LiftMechanismInstructionLogic::realTimeSleep(long long umicoroSecond)
{
    // 处理qt事件
    QCoreApplication::processEvents();

    struct timespec time;
    clock_gettime(CLOCK_REALTIME,&time);
    time.tv_nsec += umicoroSecond * 1000;
    while(time.tv_nsec >= NSEC_PER_SEC)
    {
        time.tv_nsec -= NSEC_PER_SEC;
        ++time.tv_sec;
    }
    clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&time,NULL);

}


void* LiftMechanismInstructionLogic::threadRun(void *)
{
    printf("LiftMechanismInstructionLogic theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 20;
    sched_setscheduler(0,SCHED_OTHER,&param);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
   // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
    //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消


    while(1)
    {
       sleep(2);

        while(1)
        {
            usleep(70000);
            if(true==isButtonDiValid || ENUM_CONTROL_AUTO>programEngine->getRunModel())
            {
                jogButtonMonitor();//手动模式才会生效。
            }
            if(ENUM_CONTROL_AUTO<=programEngine->getRunModel())
            {
                monitorCargo();
            }

        }
    }
    printf("LiftMechanismInstructionLogic thread quit!!!");
}

void* LiftMechanismInstructionLogic::threadTask( void* classPtr )
{
    qDebug()<<"LiftMechanismInstructionLogic thread run-------------------------";
    return ((LiftMechanismInstructionLogic*)classPtr)->threadRun(NULL);
}

void LiftMechanismInstructionLogic::createThread()
{
    int ret=pthread_create( &threadId, NULL, &LiftMechanismInstructionLogic::threadTask,this);
    qDebug()<<"LiftMechanismInstructionLogic createThread-------------------------";
}

int LiftMechanismInstructionLogic::jogButtonMonitor()
{
    if(NULL==ioController)
    {
        return 0;//等待io初始化
    }
    static bool upButtonDi_old=false;
    bool upButtonDi=ioController->getDi(mechanismConfig.upButtonDiIndex);
    static bool downButtonDi_old=false;
    bool downButtonDi=ioController->getDi(mechanismConfig.downButtonDiIndex);
    static int moveType=0;
    //上升触发
    if(true==upButtonDi && false==upButtonDi_old
            && false==downButtonDi)
    {
       qDebug()<<"按钮按下，上升触发";
       addMsg( ENUM_MSG_REMIND, "ProgramEngine", "LiftMechanismInstructionLogic",  3905, robotId );
       if(false==isInTop_block(mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index))
       {
           liftUp(mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,
                  mechanismConfig.heavyMotorReverse_index,mechanismConfig.liftDirectionExchange);
           moveType=1;
       }

    }//下降触发
    else  if(true==downButtonDi && false==downButtonDi_old
            && false==upButtonDi)
    {
        qDebug()<<"按钮按下，下降触发";
        addMsg( ENUM_MSG_REMIND, "ProgramEngine", "LiftMechanismInstructionLogic",  3906, robotId );
        if(false==isInButtom_block(mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index))
        {
            liftDown(mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index
                     ,mechanismConfig.heavyMotorReverse_index,mechanismConfig.liftDirectionExchange);
            moveType=2;
        }
    }//手动停止触发
    else  if( (false==downButtonDi && true==downButtonDi_old )
              || (false==upButtonDi && true==upButtonDi_old ))
    {
       qDebug()<<"按钮按下，手动停止触发";
       addMsg( ENUM_MSG_REMIND, "ProgramEngine", "LiftMechanismInstructionLogic",  3907, robotId );
       liftStop(mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,mechanismConfig.heavyMotorReverse_index);
       moveType=0;
    }//上限位停止触发
    else  if( 1==moveType && true== isInTop_block(mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index))
    {
       qDebug()<<"按钮按下，上限位停止触发";
       addMsg( ENUM_MSG_REMIND, "ProgramEngine", "LiftMechanismInstructionLogic",  3908, robotId );
       liftStop(mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,mechanismConfig.heavyMotorReverse_index);
       moveType=3;
    }
    //下限位停止触发
    else  if( 2==moveType && true== isInButtom_block(mechanismConfig.heavyTopSensor_index,mechanismConfig.heavyButtomSensor_index))
    {
       qDebug()<<"按钮按下，下限位停止触发";
       addMsg( ENUM_MSG_REMIND, "ProgramEngine", "LiftMechanismInstructionLogic",  3909, robotId );
       liftStop(mechanismConfig.blockType,mechanismConfig.heavyMotorPositive_index,mechanismConfig.heavyMotorReverse_index);
       moveType=4;
    }
    upButtonDi_old=upButtonDi;
    downButtonDi_old=downButtonDi;
    return 1;

}

int LiftMechanismInstructionLogic::monitorCargo()
{
    if(isInCargoMonitor && cargoSensorDiIndex>=0)
    {
        if(false==ioController->getDi(cargoSensorDiIndex))
        {
            addMsg(ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3912, robotId);
            basicStatus=E_LIFT_STATUS_ERROR;
            return 0;
        }

    }
    return 1;

}


void LiftMechanismInstructionLogic::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotId, int parameter1,
                                           int parameter2, int parameter3, int parameter4, QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;

    QString infomationStr;

    switch(messageCode)
    {
    case 3912:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("升降机构执行错误,未检测到货物感应信号！")+message;

        break;
    }
    case 3911:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（升降机构执行错误,到达底部都未检测到中间感应器）！")+message;

        break;
    }
    case 3910:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（升降机构执行错误,到达顶部都未检测到中间感应器）！")+message;

        break;
    }
    case 3909:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("　举升机构下限位开关停止触发！")+message;

        break;
    }
    case 3908:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("　举升机构上限位开关停止触发！")+message;

        break;
    }
    case 3907:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("　手动升降机构停止触发！")+message;

        break;
    }
    case 3906:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("　手动下降按钮触发！")+message;

        break;
    }
    case 3905:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("　手动举升按钮触发！")+message;

        break;
    }
    case 3904:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("　liftLogic.xml配置文件读取失败！")+message;

        break;
    }
    case 3903:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（升降机构执行超时）！")+message;

        break;
    }
    case 3902:
    {

       infomationStr = QObject::tr("机器人:") +
               QString::number(robotId) +
               QObject::tr("第") +
               QString::number(parameter1+1) +
               QObject::tr("行运行出现异常（升降机构类型错误）！")+message;

        break;
    }
        default:
        {

        }
    }

    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);
}


